# Simulation parameterswidth=640height=480duration=10# secondsframerate=120# Hz# Turtle gait parametersamplitude=30*math.pi/180# Leg swing amplitude in radiansfrequency=1# Gait frequency in Hzphase_offset=0.25# Phase difference between legs (0.25 = 90 degrees out of phase)leg_bias=60*math.pi/180# Baseline leg angle (negative = legs pointing backward)defgen_controller(frequency,amplitude,phase_offset,leg_bias):"""Generate controller function for turtle locomotion"""# We need to track accumulated phasephase_state={'right_phase':0,'left_phase':0,'last_t':0}defmy_controller(model,data):t=data.time# Calculate dtifphase_state['last_t']==0:dt=0.001# First timestepelse:dt=t-phase_state['last_t']phase_state['last_t']=t# Gradually ramp up leg movement from 0 to full amplitude over first 2 secondsift<1:current_amplitude=0else:current_amplitude=amplitude# Get current angles in cycle (0 to 2π)right_angle=phase_state['right_phase']%(2*math.pi)left_angle=phase_state['left_phase']%(2*math.pi)# Determine speed multiplier based on angle position# Fast in 1st and 4th quarters (0-90° and 270-360°)# Normal in 2nd and 3rd quarters (90-270°)# Right leg speedifright_angle<math.pi/2orright_angle>3*math.pi/2:right_speed=4.0# Double speedelse:right_speed=0.5# Normal speed# Left leg speedifleft_angle<math.pi/2orleft_angle>3*math.pi/2:left_speed=0.5# Double speedelse:left_speed=4.0# Normal speed# Update phases with variable speedphase_state['right_phase']+=right_speed*2*math.pi*frequency*dtphase_state['left_phase']+=left_speed*2*math.pi*frequency*dt# Calculate leg angles from accumulated phaseright_leg_angle=current_amplitude*math.sin(phase_state['right_phase']-2*math.pi*frequency*phase_offset)+leg_biasleft_leg_angle=current_amplitude*math.sin(phase_state['left_phase'])+leg_bias# Apply controldata.ctrl=[right_leg_angle,left_leg_angle]returnmy_controllerdefrun_sim(model,data,qpos,my_controller,show_video=True):"""Run MuJoCo simulation and capture frames"""# CREATE NEW RENDERER each timerenderer=mujoco.Renderer(model,width=width,height=height)mujoco.set_mjcb_control(my_controller)frames=[]# Data collection for trunktrunk_data={'time':[],'pos_x':[],'pos_y':[],'pos_z':[],'vel_x':[],'vel_y':[],'vel_z':[]}mujoco.mj_resetData(model,data)data.qpos=qposi=1# Get trunk body IDtrunk_id=mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_BODY,'trunk')whiledata.time<duration:mujoco.mj_step(model,data)# Collect trunk position and velocitytrunk_data['time'].append(data.time)trunk_data['pos_x'].append(data.xpos[trunk_id][0])trunk_data['pos_y'].append(data.xpos[trunk_id][1])trunk_data['pos_z'].append(data.xpos[trunk_id][2])trunk_data['vel_x'].append(data.cvel[trunk_id][3])# Linear velocitytrunk_data['vel_y'].append(data.cvel[trunk_id][4])trunk_data['vel_z'].append(data.cvel[trunk_id][5])iflen(frames)<data.time*framerate:renderer.update_scene(data,camera='target')pixels=renderer.render()frames.append(pixels)ifi==1:media.show_image(pixels)i+=1mujoco.set_mjcb_control(None)# Close renderer after userenderer.close()# Save trunk data to CSVimportpandasaspddf=pd.DataFrame(trunk_data)df.to_csv('trunk_data.csv',index=False)print(f"Trunk data saved to 'trunk_data.csv' ({len(df)} timesteps)")ifshow_video:media.show_video(frames,fps=framerate,width=width,height=height)returnframes,trunk_data
# Initialize modelxml=turtle_template.format(width=width,height=height,k=1e-5,b=1e-6)model=mujoco.MjModel.from_xml_string(xml)data=mujoco.MjData(model)renderer=mujoco.Renderer(model,width=width,height=height)# Set initial posezero_rotation=numpy.array([1,0,0,0])qpos1=numpy.array([0,0,0.05])qpos2=numpy.array([-80,-110,0,60,90])*math.pi/180#value at the 4th place is for the new joint qpos3=numpy.array([-80,-110,0,60,90])*math.pi/180#value at the 4th place is for the new joint qpos=numpy.hstack([qpos1,zero_rotation,qpos2,qpos3])# Generate controller and run simulationmy_controller=gen_controller(frequency,amplitude,phase_offset,leg_bias)frames,trunk_data=run_sim(model,data,qpos,my_controller,False)# Display final frameplt.imshow(frames[-1])plt.axis('off')plt.show()# Show animationmedia.show_video(frames,fps=framerate)
Trunk data saved to 'trunk_data.csv' (10001 timesteps)
fig,axes=plt.subplots(2,3,figsize=(15,8))# Position plotsaxes[0,0].plot(trunk_data['time'],trunk_data['pos_x'])axes[0,0].set_title('Trunk X Position')axes[0,0].set_xlabel('Time (s)')axes[0,0].set_ylabel('Position (m)')axes[0,0].grid(True)axes[0,1].plot(trunk_data['time'],trunk_data['pos_y'])axes[0,1].set_title('Trunk Y Position')axes[0,1].set_xlabel('Time (s)')axes[0,1].set_ylabel('Position (m)')axes[0,1].grid(True)axes[0,2].plot(trunk_data['time'],trunk_data['pos_z'])axes[0,2].set_title('Trunk Z Position')axes[0,2].set_xlabel('Time (s)')axes[0,2].set_ylabel('Position (m)')axes[0,2].grid(True)# Velocity plotsaxes[1,0].plot(trunk_data['time'],trunk_data['vel_x'])axes[1,0].set_title('Trunk X Velocity')axes[1,0].set_xlabel('Time (s)')axes[1,0].set_ylabel('Velocity (m/s)')axes[1,0].grid(True)axes[1,1].plot(trunk_data['time'],trunk_data['vel_y'])axes[1,1].set_title('Trunk Y Velocity')axes[1,1].set_xlabel('Time (s)')axes[1,1].set_ylabel('Velocity (m/s)')axes[1,1].grid(True)axes[1,2].plot(trunk_data['time'],trunk_data['vel_z'])axes[1,2].set_title('Trunk Z Velocity')axes[1,2].set_xlabel('Time (s)')axes[1,2].set_ylabel('Velocity (m/s)')axes[1,2].grid(True)plt.tight_layout()plt.show()
# Print everything in qpos with labelsprint("qpos (Generalized Positions):")# Free joint (trunk) - first 7 valuesprint("\nTrunk (Free Joint):")print(f" Position X: {data.qpos[0]:.6f}")print(f" Position Y: {data.qpos[1]:.6f}")print(f" Position Z: {data.qpos[2]:.6f}")print(f" Quaternion W: {data.qpos[3]:.6f}")print(f" Quaternion X: {data.qpos[4]:.6f}")print(f" Quaternion Y: {data.qpos[5]:.6f}")print(f" Quaternion Z: {data.qpos[6]:.6f}")# Joint angles - remaining valuesprint("\nJoint Angles (radians):")joint_start_idx=7# Joints start after the free joint (7 DOF)foriinrange(model.njnt):ifmodel.joint(i).type[0]!=0:# Skip free joint (already printed)joint_name=model.joint(i).nameqpos_idx=joint_start_idxangle_deg=data.qpos[qpos_idx]*180/math.piprint(f" [{qpos_idx:2d}] {joint_name:15s}: {data.qpos[qpos_idx]:8.4f} rad ({angle_deg:7.2f}°)")joint_start_idx+=1# Print totalprint(f"\nTotal qpos size: {model.nq}")print(f"Full qpos array:\n{data.qpos}")
qpos (Generalized Positions):
Trunk (Free Joint):
Position X: -0.050003
Position Y: -0.024914
Position Z: 0.033302
Quaternion W: 0.925145
Quaternion X: -0.000650
Quaternion Y: 0.002558
Quaternion Z: 0.379604
Joint Angles (radians):
[ 7] leg1_j2 : -1.3009 rad ( -74.53°)
[ 8] leg1_j3 : -2.1423 rad (-122.75°)
[ 9] leg1_j6 : -0.2239 rad ( -12.83°)
[10] leg1_j4 : 1.1481 rad ( 65.78°)
[11] leg1_j5 : 1.6928 rad ( 96.99°)
[12] leg2_j2 : -1.5006 rad ( -85.98°)
[13] leg2_j3 : -2.0335 rad (-116.51°)
[14] leg2_j6 : -1.4004 rad ( -80.24°)
[15] leg2_j4 : 0.8461 rad ( 48.48°)
[16] leg2_j5 : 1.9035 rad ( 109.06°)
Total qpos size: 17
Full qpos array:
[-5.00029135e-02 -2.49140253e-02 3.33022219e-02 9.25145310e-01
-6.49925358e-04 2.55794535e-03 3.79603991e-01 -1.30085497e+00
-2.14233212e+00 -2.23890419e-01 1.14807561e+00 1.69278308e+00
-1.50061120e+00 -2.03348547e+00 -1.40040082e+00 8.46113826e-01
1.90349143e+00]